package org.sunspotworld;

/*
 * StartApplication.java
 *
 * Created on Jul 23, 2009 2:25:38 PM;
 */


import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.peripheral.radio.RadioFactory;
import com.sun.spot.io.j2me.radiogram.*;
import com.sun.spot.peripheral.TimeoutException;
import com.sun.spot.peripheral.radio.RadioPolicyManager;
import com.sun.spot.sensorboard.peripheral.IAccelerometer3D;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.*;

import java.io.*;
import javax.microedition.io.*;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * A Robot Lazor Game
 * @author Tomas Henriquez, Sabin Larranaga
 *
 */

public class StartApplication extends MIDlet {
    public static final int START_LIFE = 5;
    public static final int CONNECT_PORT = 23;
    public static final int RUN_PORT = 20;
    public static final int SERVER_PORT = 25;
    
    public static final byte ROBOT_SIGNATURE = 4;
    public static final byte LAZER_SIGNATURE = 32;
    public static final byte CONTROL_SIGNATURE = 17;

    private static final short PID = 4;
    private static final int CHANNEL = 23;

    private int life = START_LIFE;

    public ITriColorLED[] leds = EDemoBoard.getInstance().getLEDs();
    private IAccelerometer3D accel = EDemoBoard.getInstance().getAccelerometer();
    private ISwitch sw1 = EDemoBoard.getInstance().getSwitches()[EDemoBoard.SW1];
    private ISwitchListenerImplSw1 isl = new ISwitchListenerImplSw1(this);
    private ISwitch sw2 = EDemoBoard.getInstance().getSwitches()[EDemoBoard.SW2];

    private RadiogramConnection robotConn = null;
    private RadiogramConnection lazerConn = null;
    private boolean moveFlag = false;

    public void setMoveFlag(boolean moveFlag) {
        this.moveFlag = moveFlag;
    }

    public void setLife(int life) {
        this.life = life;
    }

    public int getLife() {
        return life;
    }

    private String robotAddr = null;
    private String lazerAddr = null;
    
    private void robotConnectPieces() {
        RadiogramConnection connectConn = null;
        try {
            connectConn = (RadiogramConnection) Connector.open("radiogram://broadcast:" + CONNECT_PORT);
            Radiogram xrg = (Radiogram)connectConn.newDatagram(connectConn.getMaximumLength());
            //robotConnectrobotConn.close();
            //robotConnectrobotConn = null;
            robotConn = (RadiogramConnection)Connector.open("radiogram://:" + CONNECT_PORT);
            robotConn.setTimeout(1000);
            Radiogram rrg = (Radiogram)robotConn.newDatagram(robotConn.getMaximumLength());
            xrg.reset();
            xrg.writeByte(CONTROL_SIGNATURE);
            rrg.reset();
            //rrg.setAddress(xrg);
            byte signature;
            while (robotAddr == null || lazerAddr == null) {
                try {
                    connectConn.send(xrg);
                    robotConn.receive(rrg);
                    signature = rrg.readByte();
                    if (signature == this.ROBOT_SIGNATURE) {
                        robotAddr = rrg.getAddress();
                        System.out.println("Robot: " + robotAddr);
                    }
                    if (signature == this.LAZER_SIGNATURE) {
                        lazerAddr = rrg.getAddress();
                        System.out.println("Lazer: " + lazerAddr);
                    }
                } catch(TimeoutException tex) {

                }
            }
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        if (robotConn != null) {
            try {
                robotConn.close();
                robotConn = null;
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
        if (connectConn != null) {
            try {
                connectConn.close();
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }

    private void blinkrobotConnected() {
        for (int i=0; i<3; i++) {
            for (int j=0; j<leds.length; j++) {
                leds[j].setColor(LEDColor.GREEN);
                leds[j].setOn();
            }
            Utils.sleep(500);
            for (int j=0; j<leds.length; j++) {
                leds[j].setOff();
            }
            Utils.sleep(500);
        }
    }

    protected void startApp() throws MIDletStateChangeException {
        new BootloaderListener().start();   // monitor the USB (if robotConnected) and recognize commands from host

        RadioPolicyManager rpm = (RadioPolicyManager) RadioFactory.getRadioPolicyManager();

        rpm.setPanId(PID);
        rpm.setChannelNumber(CHANNEL);

        while (lazerAddr == null || robotAddr == null) {
            if (sw1.isClosed() && sw2.isClosed()) {
                leds[0].setColor(LEDColor.RED);
                leds[0].setOn();

                System.out.println("robotConnecting...");
                robotConnectPieces();
            } else {
                Utils.sleep(500);
            }
        }
        System.out.println("robotConnected!");

        blinkrobotConnected();

        sw1.addISwitchListener(isl);
        //Thanks to Douglas we do this while so we have a more robust remote control keeping the sign on even if an exception occurs
        while(true){

        try {
            lazerConn = (RadiogramConnection) Connector.open("radiogram://" + lazerAddr + ":"+RUN_PORT);
            sw2.addISwitchListener(new ISwitchListenerImplSw2(this, lazerConn));

            robotConn = (RadiogramConnection) Connector.open("radiogram://" + robotAddr + ":"+RUN_PORT);
            Radiogram dg = (Radiogram)robotConn.newDatagram(robotConn.getMaximumLength());
            double value_x = 0,value_y = 0;
        
            Server lifeCounter = new Server(this, robotConn);
            lifeCounter.start();

            while (life > 0) {
                //if Sw1 is pressed it enters here
                while (moveFlag == true) {
                    dg.reset();
                    value_x = validation(accel.getAccelX());

                    if (value_x >= 0) {
                        value_x = value_x + 5;
                    } else {
                        value_x = value_x - 5;
                    }

                    dg.writeDouble(value_x);
                    robotConn.send(dg);
                    System.out.println("Forward!!!");
                    Utils.sleep(250);

                }
                //if not it goes thru here
                dg.reset();
                value_x = validation(accel.getAccelX()) * -1;
                value_y = accel.getAccelY();
                
                if(value_y < -0.8){ //It sends a sign to make the robot goes backwards
                    dg.writeDouble(Double.MIN_VALUE);
                } 
                else if (value_x < 0.1 && value_x > -0.1) { //ROBOT STOP!! 
                    dg.writeDouble(0);
                } else {
                    dg.writeDouble(value_x); //writes the normal values
                }

                robotConn.send(dg);
                System.out.println("Hey, turn...maybe");
                Utils.sleep(250);
            
            }
        } catch (IOException ex) {
            ex.printStackTrace();
        }
        //Thanks to douglas we assure here we dont try to close the robotConnection while it is null
        //so we made a "semaphore" whit a tiny help from java :p
        synchronized(this){
         {
            try {
                if (robotConn != null) {
                    robotConn.close();
                    robotConn = null;
                }
            } catch (IOException ex) {
                ex.printStackTrace();
            }

        }
        }
      }
    }


    /**
     *  hew we send a sign so the motor turns off
     *  
     * @param 
     * @return
     */ 
    private synchronized void setOff() {
        
        try {
            robotConn = (RadiogramConnection) Connector.open("radiogram://" + robotAddr + ":"+RUN_PORT);
            Datagram dg = robotConn.newDatagram(robotConn.getMaximumLength());
            dg.writeDouble(Double.MAX_VALUE);
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }

    /**
     *  We asure the max and min values we can take from X in the acelerometer
     * @param value
     * @return a value between 1 and -1
     */
    private double validation(double value) {
        if (value > 1) {
            return 1;
        }
        if (value < -1) {
            return -1;
        }

        return value;
    }

    private void ledsOff(){
        for (int i = 0; i < leds.length; i++) {
            leds[i].setOff();
        }
    }

    protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     *
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {
        ledsOff();
        setOff();
    }
}
